#include "pid.h"

PID::PID()//constructor
{
    G_targetLocation=0.0;
    G_presentLocation=0.0;
    Kp=0, Ki=0, Kd=0;
    error_p=0;
    error_i=0;
    error_d=0;
    error_dp=0;
    cur_error=100;//construct
}

void PID::PID_Update()
{
    if(the_1st_update)
    {
        error_dp=G_targetLocation-G_presentLocation;
        the_1st_update=false;
    }
    error_p = G_targetLocation - G_presentLocation;
    error_i += error_p;
    error_d = error_p - error_dp;
    error_dp = error_p;
    G_presentLocation += Kp * error_p + Ki * error_i + Kd * error_d;
    cur_error=G_targetLocation - G_presentLocation;
    qDebug()<<"current position:" << G_presentLocation << "current error:" <<cur_error;
    return;
}
